﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace CoreRay
{
    public class GridSpatial : SpatialStructure
    {
        private GeometryObjectRayCache[] geometryObjectCacheEntries;
        private GridVoxel[] voxels;
        private int voxelCountX;
        private int voxelCountY;
        private int voxelCountZ;
        private Vector3 voxelSize;
        private Vector3 inverseVoxelSize;
        int currentRayId;

        private StatisticsItemMemory memoryStat;

        public GridSpatial(bool useWorldMatrix) : this(useWorldMatrix, 1000)
        {

        }

        public GridSpatial(bool useWorldMatrix, int maxVoxelCount) : base(useWorldMatrix)
        {
            this.MaxVoxelCount = maxVoxelCount;
        }

        public int MaxVoxelCount
        {
            get;
            private set;
        }

        public override void Construct(List<GeometryObject> geometryObjects)
        {
            //Init statistics
            string statisticsCategory = "Grid Spatial - " + this.Name;

            this.memoryStat = new StatisticsItemMemory("Memory Allocated");
            CoreRayContext.Current.Statistics.AddStatisticsItem(statisticsCategory, this.memoryStat);

            this.memoryStat.RecordMemoryUsageChange();

            //Construct the geometry object cache entries and the bounding box
            this.geometryObjectCacheEntries = new GeometryObjectRayCache[geometryObjects.Count];
            this.BoundingBox = new BoundingBox();

            for (int i = 0; i < geometryObjects.Count; i++)
            {
                this.geometryObjectCacheEntries[i] = new GeometryObjectRayCache(geometryObjects[i]);
                this.BoundingBox.Add(this.UseWorldMatrix ? geometryObjects[i].WorldBoundingBox : geometryObjects[i].BoundingBox);
            }

            //Compute boundingbox delta
            Vector3 boundingBoxDelta = this.BoundingBox.MaxPoint - this.BoundingBox.MinPoint;

            //Find voxelsPerUnitDist for grid
            float deltaMaxAxis;

            if (boundingBoxDelta.X > boundingBoxDelta.Y && boundingBoxDelta.X > boundingBoxDelta.Z)
            {
                deltaMaxAxis = boundingBoxDelta.X;
            }

            else if (boundingBoxDelta.Y > boundingBoxDelta.Z)
            {
                deltaMaxAxis = boundingBoxDelta.Y;
            }

            else
            {
                deltaMaxAxis = boundingBoxDelta.Z;
            }

            float inverseMaxWidth = 1.0f / deltaMaxAxis;
	        float cubeRoot = 3.0f * (float) Math.Pow((float) geometryObjects.Count, 1.0f / 3.0f);
            float voxelsPerUnitDistance = cubeRoot * inverseMaxWidth;

            //Compute voxel size for each dimension
            this.voxelCountX = (int)(boundingBoxDelta.X * voxelsPerUnitDistance);
            this.voxelCountX = MathUtility.Clamp(this.voxelCountX, 1, this.MaxVoxelCount);

            this.voxelCountY = (int)(boundingBoxDelta.Y * voxelsPerUnitDistance);
            this.voxelCountY = MathUtility.Clamp(this.voxelCountY, 1, this.MaxVoxelCount);

            this.voxelCountZ = (int)(boundingBoxDelta.Z * voxelsPerUnitDistance);
            this.voxelCountZ = MathUtility.Clamp(this.voxelCountZ, 1, this.MaxVoxelCount);

            //Compute voxel widths
            this.voxelSize = new Vector3(boundingBoxDelta.X / voxelCountX, boundingBoxDelta.Y / voxelCountY, boundingBoxDelta.Z / voxelCountZ);
            this.inverseVoxelSize = new Vector3((this.voxelSize.X == 0.0f) ? 0.0f : 1.0f / this.voxelSize.X, (this.voxelSize.Y == 0.0f) ? 0.0f : 1.0f / this.voxelSize.Y, (this.voxelSize.Z == 0.0f) ? 0.0f : 1.0f / this.voxelSize.Z);

            //Allocate voxel array
            this.voxels = new GridVoxel[this.voxelCountX * this.voxelCountY * this.voxelCountZ];

            //Add primitives to allocated voxels
            for (int i = 0; i < this.geometryObjectCacheEntries.Length; i++)
            {
                //Get the boundingBox of the geometry object
                BoundingBox objectBoundingBox = this.UseWorldMatrix ? this.geometryObjectCacheEntries[i].GeometryObject.WorldBoundingBox : this.geometryObjectCacheEntries[i].GeometryObject.BoundingBox;

                //Transform the min and max points of the bounding box to voxel coordinates
                int[] minPointVoxelCoordinates = PositionToVoxel(objectBoundingBox.MinPoint);
                int[] maxPointVoxelCoordinates = PositionToVoxel(objectBoundingBox.MaxPoint);

                // Add primitive to overlapping voxels
		        for (int z = minPointVoxelCoordinates[2]; z <= maxPointVoxelCoordinates[2]; z++)
                {
			        for (int y = minPointVoxelCoordinates[1]; y <= maxPointVoxelCoordinates[1]; y++)
                    {
				        for (int x = minPointVoxelCoordinates[0]; x <= maxPointVoxelCoordinates[0]; x++) 
                        {
                            int offset = ComputVoxelOffset(x, y, z);

                            if (this.voxels[offset] == null)
                            {
                                this.voxels[offset] = new GridVoxel();
                            }

                            this.voxels[offset].GeometryObjectCacheEntries.Add(this.geometryObjectCacheEntries[i]);
				        }
                    }
                }
            }

            this.memoryStat.RecordMemoryUsageChange();
        }

        public override Intersection Intersect(Ray ray)
        {
            Intersection intersection = null;

            //Check to see if the ray intersect the bounding box
            float rayT;

            if (this.BoundingBox.Contains(ray.ComputePoint(ray.MinimumT)))
            {
                rayT = ray.MinimumT;
            }

            else
            {
                rayT = this.BoundingBox.IntersectDistance(ray);

                if (rayT == 0.0f)
                {
                    return null;
                }
            }

            //Compute the intersection point if it intersect
            Vector3 gridIntersection = ray.ComputePoint(rayT);

            //Compute the ray id
            var rayId = currentRayId++;

            //Convert the intersection point to voxel coordinates
            var voxelCoordinates = PositionToVoxel(gridIntersection);

            //Setup 3D DDA for ray
            var nextCrossing = new float[3];
            var delta = new float[3];
            var step = new int[3];
            var output = new int[3];

            for (int i = 0; i < 3; i++)
            {
                if (ray.Direction[i] >= 0)
                {
                    //Handle ray with positive direction for voxel stepping
                    nextCrossing[i] = rayT + (VoxelToPosition(voxelCoordinates[i] + 1, i) - gridIntersection[i]) / ray.Direction[i];
                    delta[i] = this.voxelSize[i] / ray.Direction[i];
                    step[i] = 1;

                    if (i == 0)
                    {
                        output[i] = this.voxelCountX;
                    }

                    else if (i == 1)
                    {
                        output[i] = this.voxelCountY;
                    }

                    else
                    {
                        output[i] = this.voxelCountZ;
                    }
                }

                else
                {
                     //Handle ray with negative direction for voxel stepping
                    nextCrossing[i] = rayT + (VoxelToPosition(voxelCoordinates[i], i) - gridIntersection[i]) / ray.Direction[i];
                    delta[i] = -this.voxelSize[i] / ray.Direction[i];
                    step[i] = -1;
                    output[i] = -1;
                }
            }


            // Walk ray through voxel grid
            //bool hitSomething = false;
            //for (;;) {
            //    Voxel *voxel =
            //        voxels[Offset(Pos[0],	Pos[1], Pos[2])];
            //    if (voxel != NULL)
            //        hitSomething |= voxel->Intersect(ray, isect, rayId);
            //    // Advance to next voxel
            //    // Find _stepAxis_ for stepping to next voxel
            //    int bits = ((NextCrossingT[0] < NextCrossingT[1]) << 2) +
            //        ((NextCrossingT[0] < NextCrossingT[2]) << 1) +
            //        ((NextCrossingT[1] < NextCrossingT[2]));
            //    const int cmpToAxis[8] = { 2, 1, 2, 1, 2, 2, 0, 0 };
            //    int stepAxis = cmpToAxis[bits];
            //    if (ray.maxt < NextCrossingT[stepAxis])
            //        break;
            //    Pos[stepAxis] += Step[stepAxis];
            //    if (Pos[stepAxis] == Out[stepAxis])
            //        break;
            //    NextCrossingT[stepAxis] += DeltaT[stepAxis];
            //}

            return intersection;
        }

        public override bool IntersectPosition(Ray ray)
        {
            throw new NotImplementedException();
        }

        private int[] PositionToVoxel(Vector3 position)
        {
            var result = new int[3];
            
            result[0] = MathUtility.Clamp((int)((position.X - this.BoundingBox.MinPoint.X) * this.inverseVoxelSize.X), 0, this.voxelCountX - 1);
            result[1] = MathUtility.Clamp((int)((position.Y - this.BoundingBox.MinPoint.Y) * this.inverseVoxelSize.Y), 0, this.voxelCountY - 1);
            result[2] = MathUtility.Clamp((int)((position.Z - this.BoundingBox.MinPoint.Z) * this.inverseVoxelSize.Z), 0, this.voxelCountZ - 1);

            return result;
        }

        private Vector3 VoxelToPosition(int x, int y, int z) 
        {
            return this.BoundingBox.MinPoint + new Vector3(x * this.voxelSize.X, y * this.voxelSize.Y, z * this.voxelSize.Z);
	    }

        float VoxelToPosition(int p, int axis)
        {
            return this.BoundingBox.MinPoint[axis] + p * this.voxelSize[axis];
	    }

        private int ComputVoxelOffset(int x, int y, int z) 
        {
            return x + y * this.voxelCountX + z * this.voxelCountX * this.voxelCountY;
	    }
    }
}
